import log.Log;

import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;


public class AIProjectMain {
	
	private static final String LOG_TAG = AIProjectMain.class.getSimpleName();
		
	public static void main(String[] args){
		Robot r = new Robot();
		Path p = new Path("./json/Path-from-bed.json");
		int positionGoal = 0; 
		while(p.getPath().size() > 0){
			Log.i(LOG_TAG, "List size : " + p.getPath().size());
			r.getCurrentPose();
			//r.calulateLookAheadDistance(p);
			positionGoal = p.getNextGoalPoint(r.getLookAheadDistance(), r);
			Vector3D goalPoint = p.getPath().get(positionGoal).convertPositionToRobotCoordinateSystem(r);
			double angle = Math.atan2(goalPoint.getY(), goalPoint.getX());
			
			r.purePursuit(angle, goalPoint);
			
			for(int i = positionGoal; i >= 0; i--){
				p.getPath().remove(0);
			}
		}
	}
}

